solver_manager.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// wolf includes
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "core/factor/factor_base.h"
#include "core/solver/factory_solver.h"
#include "core/common/profiling_unit.h"
 
namespace wolf
{
WOLF_PTR_TYPEDEFS(SolverManager)
 
/*
 * Macro for defining Autoconf solver creator for WOLF's high level API.
 *
 * Place a call to this macro inside your class declaration (in the solver_class.h file),
 * preferably just after the constructors. Inside 'public' keyword.
 *
 * In order to use this macro, the derived solver manager class, SolverClass,
 * must have a constructor available with the API:
 *
 *   SolverClass(const ProblemPtr& wolf_problem,
 *                      const YAML::Node _params);
 *
 * Also, there should be the schema file 'SolverClass.schema' containing the specifications
 * of the user input yaml file.
 *
 * NOTE: We use `new` and not `make_shared` since the constructor may (should) be private/protected
 */
#define WOLF_SOLVER_CREATE(SolverClass)                                                                               \
    static SolverManagerPtr create(const ProblemPtr&               _problem,                                          \
                                   const YAML::Node&               _input_node,                                       \
                                   const std::vector<std::string>& _folders_schema = {})                              \
    {                                                                                                                 \
        if (not _folders_schema.empty())                                                                              \
        {                                                                                                             \
            auto server = yaml_schema_cpp::YamlServer(_folders_schema);                                               \
            server.setYaml(_input_node);                                                                              \
            if (not server.applySchema(#SolverClass))                                                                 \
            {                                                                                                         \
                WOLF_ERROR(server.getLog());                                                                          \
                return nullptr;                                                                                       \
            }                                                                                                         \
            return std::shared_ptr<SolverClass>(new SolverClass(_problem, server.getNode()));                         \
        }                                                                                                             \
        return std::shared_ptr<SolverClass>(new SolverClass(_problem, _input_node));                                  \
    }                                                                                                                 \
    static SolverManagerPtr create(const ProblemPtr&               _problem,                                          \
                                   const std::string&              _yaml_filepath,                                    \
                                   const std::vector<std::string>& _folders_schema)                                   \
    {                                                                                                                 \
        auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);                                   \
        if (not server.applySchema(#SolverClass))                                                                     \
        {                                                                                                             \
            WOLF_ERROR(server.getLog());                                                                              \
            return nullptr;                                                                                           \
        }                                                                                                             \
        return create(_problem, server.getNode(), {});                                                                \
    }
 
class SolverManager
{
  public:
    enum class CovarianceBlocksToBeComputed : std::size_t
    {
        ALL             = 0,  
        ALL_MARGINALS   = 1,  
        ROBOT_LANDMARKS = 2,  
        GAUSS                     = 3,  
        GAUSS_TUCAN               = 4,  
        GAUSS_TUCAN_ONLY_POSITION = 5   
    };
 
    enum class ReportVerbosity : std::size_t
    {
        QUIET = 0,
        BRIEF = 1,
        FULL  = 2
    };
 
    // PROFILING
    void                 printProfiling(std::ostream& stream = std::cout) const;
    const ProfilingUnit& getProfilingTotal() const;
    const ProfilingUnit& getProfilingSolve() const;
    const ProfilingUnit& getProfilingUpdate() const;
    const ProfilingUnit& getProfilingState() const;
    const ProfilingUnit& getProfilingCov() const;
 
  protected:
    ProblemPtr    wolf_problem_;
    ProfilingUnit profiling_total_, profiling_solve_, profiling_update_, profiling_state_, profiling_cov_;
    YAML::Node    params_;
 
  private:
    std::thread solver_thread_;
 
  public:
    SolverManager(const ProblemPtr& _problem);
    SolverManager(const ProblemPtr& _problem, const YAML::Node& _params);
 
    static SolverManagerPtr autoSetup(const ProblemPtr&        _problem,
                                      const std::string&       _input_yaml_file,
                                      std::vector<std::string> _primary_schema_folders,
                                      bool                     _skip_yaml_validation = false);
 
    static SolverManagerPtr autoSetup(const ProblemPtr&        _problem,
                                      YAML::Node               _param_node,
                                      std::vector<std::string> _primary_schema_folders,
                                      bool                     _skip_yaml_validation = false);
 
    virtual ~SolverManager();
 
    std::string solve();
    std::string solve(const ReportVerbosity report_level);
 
    virtual bool computeCovariances() final;
 
    virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
 
    virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
 
    virtual void update();
 
    ProblemPtr getProblem();
 
    YAML::Node getParams() const;
 
    bool getComputeCov() const;
 
    double getPeriod() const;
 
    double getCovPeriod() const;
 
    ReportVerbosity getVerbosity() const;
 
    virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
 
    virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
 
    virtual bool isStateBlockFixed(const StateBlockPtr& st) const final;
 
    virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
 
    virtual bool hasThisLocalParametrization(const StateBlockPtr&               st,
                                             const LocalParametrizationBasePtr& local_param) const final;
 
    virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
 
    virtual int numFactors() const final;
    virtual int sizeFactors() const final;
    virtual int numStateBlocks() const final;
    virtual int sizeStateBlocks() const final;
    virtual int numStateBlocksEstimated() const final;
    virtual int sizeStateBlocksEstimated() const final;
 
    virtual int numFactorsDerived() const               = 0;
    virtual int sizeFactorsDerived() const              = 0;
    virtual int numStateBlocksEstimatedDerived() const  = 0;
    virtual int sizeStateBlocksEstimatedDerived() const = 0;
 
    virtual bool check(std::string prefix = "") const final;
 
  protected:
    std::map<StateBlockPtr, Eigen::VectorXd>   state_blocks_;
    std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
    std::set<FactorBasePtr>                    factors_;
    std::set<StateBlockPtr>                    floating_state_blocks_;
 
    virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
    const double*            getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
    double*                  getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
 
  private:
    // SolverManager functions
    virtual void addFactor(const FactorBasePtr& fac_ptr) final;
    virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
    virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
    virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
    virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
    virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
    virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
 
    // Pure virtual functions to be derived
  protected:
    virtual std::string solveDerived(const ReportVerbosity report_level)                     = 0;
    virtual bool        computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
    virtual bool        computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
 
    virtual void addFactorDerived(const FactorBasePtr& fac_ptr)                              = 0;
    virtual void removeFactorDerived(const FactorBasePtr& fac_ptr)                           = 0;
    virtual void addStateBlockDerived(const StateBlockPtr& state_ptr)                        = 0;
    virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr)                     = 0;
    virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)               = 0;
    virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
 
    virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const                      = 0;
    virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const                            = 0;
    virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) const                                  = 0;
    virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const                            = 0;
    virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr&               st,
                                                    const LocalParametrizationBasePtr& local_param) const = 0;
 
    virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
    virtual bool checkDerived(std::string prefix = "") const                   = 0;
 
  public:
    virtual bool         converged() const   = 0;
    virtual bool         failed() const      = 0;
    virtual bool         wasStopped() const  = 0;
    virtual unsigned int iterations() const  = 0;
    virtual double       initialCost() const = 0;
    virtual double       finalCost() const   = 0;
    virtual double       totalTime() const   = 0;
 
    virtual void run() final;
    virtual void stop() final;
 
  private:
    virtual void loop();
 
  protected:
    // PARAMS
    double                                         period_;
    ReportVerbosity                                verbose_;
    bool                                           compute_cov_;
    CovarianceBlocksToBeComputed                   cov_enum_;
    double                                         cov_period_;
    std::chrono::high_resolution_clock::time_point last_cov_time_;
    bool                                           stopping_;
};
 
}  // namespace wolf